#include "fuzzy.hpp"

// 模糊化处理（将精确值转换为模糊量）
// 在模糊化函数中添加保护
float FuzzyPID::fuzzyfication(float value, float max)
{
    if (max < 1e-6)
        return 0.0f; // 防止除零
    float normalized = value / max;
    return std::max(-3.0f, std::min(3.0f, normalized * 3.0f));
}

// 添加隶属度计算
void FuzzyPID::calculateMembership(float value, float *membership)
{
    const float centers[7] = {-3, -2, -1, 0, 1, 2, 3};
    const float width = 1.2f; // 重叠宽度
    for (int i = 0; i < 7; ++i)
    {
        float distance = fabs(value - centers[i]);
        membership[i] = std::max(0.0f, 1.0f - distance / width);
    }
}

// 解模糊处理（重心法）
float FuzzyPID::defuzzification(float *membership)
{
    const float centers[7] = {-3, -2, -1, 0, 1, 2, 3};
    float numerator = 0.0f;
    float denominator = 0.0f;
    
    for (int i = 0; i < 7; ++i)
    {
        numerator += membership[i] * centers[i];
        denominator += membership[i];
    }
    
    return denominator > 1e-6 ? numerator / denominator : 0.0f;
}

float FuzzyPID::update(float current_value, float target)
{
    // 1. 计算误差
    float error = target - current_value;
    float delta_error = error - prev_error;

    // 2. 模糊推理（需要完善规则匹配）
    float e = fuzzyfication(error, error_max);
    float ec = fuzzyfication(delta_error, delta_error_max);

    // 3. 隶属度计算
    float e_mem[7], ec_mem[7];
    calculateMembership(e, e_mem);
    calculateMembership(ec, ec_mem);

    // 4. 模糊推理
    float kp_delta = 0.0f, ki_delta = 0.0f, kd_delta = 0.0f;
    float total_activation = 0.0f;
    for (int i = 0; i < 7; ++i)
    {
        for (int j = 0; j < 7; ++j)
        {
            float activation = std::min(e_mem[i], ec_mem[j]);
            kp_delta += activation * kp_rule[i][j] * kp_max*0.3f;
            ki_delta += activation * ki_rule[i][j] * ki_max;
            kd_delta += activation * kd_rule[i][j] * kd_max;
            total_activation += activation;
        }
    }

    // 归一化
    if (total_activation > 1e-6)
    {
        kp_delta /= total_activation;
        ki_delta /= total_activation;
        kd_delta /= total_activation;
    }

    // 5. 参数更新（添加限幅）
    kp = std::max(base_kp * 0.1f, std::min(base_kp * 5.0f, kp + kp_delta));
    ki = std::max(base_ki * 0.01f, std::min(base_ki * 2.0f, ki + ki_delta));
    kd = std::max(base_kd * 0.0f, std::min(base_kd * 3.0f, kd + kd_delta));

    // 6. PID计算（添加积分限幅）
    integral += error;
    float integral_limit = (ki > 0.01f) ? (error_max * 0.5f) / ki : error_max * 50.0f; // 根据当前ki值动态调整
    integral = std::max(-integral_limit, std::min(integral_limit, integral));

    float output = kp * error + ki * integral + kd * delta_error;
    prev_error = error;

    return output;
}